#include "usart.h"
#include "cmsis_os.h"

#include "chassis.h"
#include "robot_def.h"

#include "general_def.h"
#include "bsp_dwt.h"
#include "arm_math.h"
#include "bsp_pwm.h"
#include "pwm_motor.h"
#include "tim.h"
#include "mpu6500.h"
#include "message_center.h"
#include "dji_motor.h"
#include "servo_motor.h"
#include "zdt_usart.h"

#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长
#define MOTOR_ACC 0  // 轮子加速度


static PWM_MOTOR_Instance_s *PWM_Motor_R, *PWM_Motor_L;
static DJIMotorInstance *m2006;
static ZDTInstance *Motor_LF, *Motor_RF, *Motor_LB, *Motor_RB;

static attitude_t *gimba_IMU_data; // 云台IMU数据
static Publisher_t *chassis_pub;  // 用于发布底盘的数据
static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令
static float chassis_vx, chassis_vy, chassis_wz;     // 将云台系的速度投影到底盘
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
static Chassis_Offset chassis;
static float max_vel = 50;
static uint32_t CNT; 

#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)

#define ChassisWidth 12.0f
#define ChassisRadius (ChassisWidth / 2f)
#define WheelRadius 3.25f

static void RPM_2_VEL(float *rpm)
{
    *rpm = *rpm * PI2 * RADIUS_WHEEL / 60;
}


void ChassisInit()
{
    HAL_GPIO_WritePin(GPIOG, D8_Pin | D7_Pin | D6_Pin | D5_Pin | D4_Pin | D3_Pin | D2_Pin | D1_Pin, GPIO_PIN_SET);
    // gimba_IMU_data = INS_Init();
    PWM_MOTOR_Init_Config_s PWM_Motor_L_Config = {
        .pwm_init_config = {
            .channel = TIM_CHANNEL_1,
            .dutyratio = 0.5,
            .htim = &htim2,
            .period = 1,
        },
        .dir_gpio_1 = {
            .gpio_port = GPIOC,
            .gpio_pin = GPIO_PIN_2,
        },
        .dir_gpio_2 = {
            .gpio_port = GPIOC,
            .gpio_pin = GPIO_PIN_3,
        },
        .close_loop_type = ANGLE_LOOP,
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 0.75,
                .Ki = 0,
                .Kd = 0.015,
                .MaxOut = 100,
            },
            .speed_PID = {
                .Kp = 10,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 100,
            },
            .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
        },
        .angle_feedback_source = MOTOR_FEED,
        .control_type = PWM_MOTOR_CLOSELOOP,
        .direction = PWM_MOTOR_DIRECTION_NORMAL,
        .encoder_tim = &htim8,
        .angle_feedback_dir = FEEDBACK_DIRECTION_NORMAL,
        .speed_feedback_dir = FEEDBACK_DIRECTION_NORMAL,
    };
    PWM_Motor_L = PWMMotorInit(&PWM_Motor_L_Config);
    PWM_MOTOR_Init_Config_s PWM_Motor_R_Config = {
        .pwm_init_config = {
            .channel = TIM_CHANNEL_1,
            .dutyratio = 0.5,
            .htim = &htim4,
            .period = 1,
        },
        .dir_gpio_1 = {
            .gpio_port = GPIOE,
            .gpio_pin = GPIO_PIN_5,
        },
        .dir_gpio_2 = {
            .gpio_port = GPIOE,
            .gpio_pin = GPIO_PIN_6,
        },
        .close_loop_type = ANGLE_LOOP,
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 0.75,
                .Ki = 0,
                .Kd = 0.015,
                .MaxOut = 100,
            },
            .speed_PID = {
                .Kp = 10,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 100,
            },
            .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
        },
        .control_type = PWM_MOTOR_CLOSELOOP,
        .direction = PWM_MOTOR_DIRECTION_REVERSE,
        .encoder_tim = &htim5,
        .angle_feedback_source = MOTOR_FEED,
        .angle_feedback_dir = FEEDBACK_DIRECTION_NORMAL,
        .speed_feedback_dir = FEEDBACK_DIRECTION_NORMAL,
    };
    PWM_Motor_R = PWMMotorInit(&PWM_Motor_R_Config);
    Motor_Init_Config_s m2006_config = {
        .can_init_config  = {
            .can_handle = &hcan1,
            .tx_id = 1
        },
        .motor_type = M2006,
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 0,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 100,
            },
            .speed_PID ={
                .Kp = 1,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 2000,
            },
            },
            .controller_setting_init_config={
                .close_loop_type = SPEED_LOOP,
                .outer_loop_type = SPEED_LOOP,
                .speed_feedback_source = MOTOR_FEED,
                .angle_feedback_source = MOTOR_FEED,
            }
        };

    PID_Init_Config_s pid_init = {
        .Kp = 2,
        .Ki = 0.1,
        .Kd = 0,
        .MaxOut = 80,
        .DeadBand = 0,
        .IntegralLimit = 25,
        .Improve = PID_Integral_Limit | PID_DerivativeFilter | PID_OutputFilter,
        .Output_LPF_RC = 0.005,
    };

    PID_Init_Config_s wz_pid_init = {
        .Kp = 3,
        .Ki = 0.01,
        .Kd = 0,
        .MaxOut = 20,
        .DeadBand = 0,
        .IntegralLimit = 3,
        .Improve = PID_Integral_Limit,
    };

    // memset(&chassis.pid, 0, sizeof(PIDInstance));


    m2006 = DJIMotorInit(&m2006_config);

    ZDT_Motor_Init_Config_s Zdt_Motor_Init = {
        .id = 1,
    };
    Motor_LF = ZDTMotorUSARTInit(&huart8, Zdt_Motor_Init);
    Zdt_Motor_Init.id = 2;
    Motor_RF = ZDTMotorUSARTInit(&huart8, Zdt_Motor_Init);
    Zdt_Motor_Init.id = 3;
    Motor_RB = ZDTMotorUSARTInit(&huart8, Zdt_Motor_Init);
    Zdt_Motor_Init.id = 4;
    Motor_LB = ZDTMotorUSARTInit(&huart8, Zdt_Motor_Init);

    ZDT_En_Control(Motor_LF, true, false);
    ZDT_En_Control(Motor_RF, true, false);
    ZDT_En_Control(Motor_LB, true, false);
    ZDT_En_Control(Motor_RB, true, false);

    // chassis = malloc(sizeof(Chassis_Offset)); // 分配内存
    // if (chassis != NULL) {
    //     memset(chassis, 0, sizeof(chassis));  // 初始化内存为零
    // } else {
    //     while(1);  
    // }

   chassis.c_xy[0] = 100;
   chassis.c_xy[1] = 100;

   chassis.a_xy[0] = 1600;
   chassis.a_xy[1] = 100;
    
    RPM_2_VEL(&max_vel);
    PIDInit(&chassis.pid_vx, &pid_init);
    PIDInit(&chassis.pid_vy, &pid_init);
    PIDInit(&chassis.pid_wz, &wz_pid_init);
    chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
}

/**
 * @brief 计算每个轮毂电机的输出,正运动学解算
 *        用宏进行预替换减小开销,运动解算具体过程参考教程
 */
static void MecanumCalculate()
{
    vt_lf = -chassis_vy - chassis_vx - chassis_wz * LF_CENTER;
    vt_rf = -chassis_vy + chassis_vx - chassis_wz * RF_CENTER;
    vt_lb = chassis_vy - chassis_vx - chassis_wz * LB_CENTER;
    vt_rb = chassis_vy + chassis_vx - chassis_wz * RB_CENTER;
}

static void ChassisControl(uint8_t acc)
{
    // chassis_vx = vx;
    // chassis_vy = vy;
    // chassis_wz = wz;
    MecanumCalculate();

    ZDT_Vel_Send(Motor_LF, vt_lf, acc);
    ZDT_Vel_Send(Motor_RF, vt_rf, acc);
    ZDT_Vel_Send(Motor_LB, vt_lb, acc);
    ZDT_Vel_Send(Motor_RB, vt_rb, acc);
}

static void ZDT_Read()
{
    ZDT_Read_Sys_Params(Motor_LF, S_VEL);
    ZDT_Read_Sys_Params(Motor_RF, S_VEL);
    ZDT_Read_Sys_Params(Motor_LB, S_VEL);
    ZDT_Read_Sys_Params(Motor_RB, S_VEL);

    ZDT_Read_Sys_Params(Motor_LF, S_CPOS);
    ZDT_Read_Sys_Params(Motor_RF, S_CPOS);
    ZDT_Read_Sys_Params(Motor_LB, S_CPOS);
    ZDT_Read_Sys_Params(Motor_RB, S_CPOS);

    ZDT_Angle_Calc(Motor_LF,  1);
    ZDT_Angle_Calc(Motor_RF, -1);
    ZDT_Angle_Calc(Motor_LB,  1);
    ZDT_Angle_Calc(Motor_RB, -1);

}

static int ZDT_Pos_reset()
{
    static int reset_flag[4] = {0};

    if(reset_flag[0] == 0){
        ZDT_Reset_CurPos_To_Zero(Motor_LF);
        if(Motor_LF->state.CurrentPosition == 0)
            reset_flag[0] = 1;
    }

    if(reset_flag[1] == 0){
        ZDT_Reset_CurPos_To_Zero(Motor_RF);
        if(Motor_RF->state.CurrentPosition == 0)
            reset_flag[1] = 1;
    }

        if(reset_flag[2] == 0){
        ZDT_Reset_CurPos_To_Zero(Motor_LB);
        if(Motor_LB->state.CurrentPosition == 0)
            reset_flag[2] = 1;
    }

        if(reset_flag[3] == 0){
        ZDT_Reset_CurPos_To_Zero(Motor_RB);
        if(Motor_RB->state.CurrentPosition == 0)
            reset_flag[3] = 1;
    }

    if((reset_flag[0] && reset_flag[1] && reset_flag[2] && reset_flag[3]) == 1)
        return 1;
} 


static void MotorPosCalc(ZDTInstance *zdt, int x)
{
    // Displacement *dis = &(zdt->displacement);  // 多次使用时，保存指针减小内存开销
    
    zdt->displacement.s = zdt->state.rad * RADIUS_WHEEL;
    zdt->displacement.temp_x = zdt->displacement.s * SQUARE_ROOT_2_DIV_2;
    zdt->displacement.temp_y = zdt->displacement.s * SQUARE_ROOT_2_DIV_2;

    zdt->displacement.error_x = zdt->displacement.temp_x - zdt->displacement.last_temp_x;
    zdt->displacement.error_y = zdt->displacement.temp_y - zdt->displacement.last_temp_y;
    
    zdt->displacement.x += zdt->displacement.error_x;
    zdt->displacement.y += zdt->displacement.error_y;

    // zdt->state.angle *= x;
    // zdt->state.rad   *= x;

    zdt->displacement.last_temp_x = zdt->displacement.x;
    zdt->displacement.last_temp_y = zdt->displacement.y;
}


static void MecanumPosControl()
{
    float error_x, error_y, error_x2, error_y2; // 当前坐标与目标坐标的差
    float displacement, displacement_2; // temp_displacement_2 是当前坐标指向目标坐标向量的模长的平方

    MotorPosCalc(Motor_LF, 1);
    MotorPosCalc(Motor_RF, -1);
    MotorPosCalc(Motor_LB, 1);
    MotorPosCalc(Motor_RB, -1);


    
   chassis.x = (Motor_LF->displacement.x + Motor_RF->displacement.x + Motor_LB->displacement.x + Motor_RB->displacement.x) * 0.25;
   chassis.y = (-Motor_LF->displacement.y + Motor_RF->displacement.y + Motor_LB->displacement.y - Motor_RB->displacement.y) * 0.25;
   chassis.error_x =chassis.x -chassis.last_x;
   chassis.error_y =chassis.y -chassis.last_y;
   chassis.last_x =chassis.x;
   chassis.last_y =chassis.y; 
   chassis.c_xy[0] +=chassis.error_x;
   chassis.c_xy[1] +=chassis.error_y;

    error_x = -chassis.a_xy[0] +chassis.c_xy[0];
    error_y = -chassis.a_xy[1] +chassis.c_xy[1];
    error_x2 = powf(error_x, 2);
    error_y2 = powf(error_y, 2);
   chassis.displacement_2 = error_x2 + error_y2;
   chassis.displacement = sqrtf(chassis.displacement_2);

    //chassis.start_flag[0] = (error_x < error_y) ? 1 : 0;
    //chassis.start_flag[1] = (error_x > error_y) ? 1 : 0;

        chassis_vx = PIDCalculate(&chassis.pid_vx, error_x, 0);
        chassis_vy = -PIDCalculate(&chassis.pid_vy, error_y, 0);
        chassis_wz = -PIDCalculate(&chassis.pid_wz, gimba_IMU_data->Yaw, 0);
        CNT++;

}


// 陀螺仪顺时针为负
void ChassisTask()
{   
    static uint8_t Init_flag = 0;
    
    SubGetMessage(chassis_sub, &chassis_cmd_recv);
    
    ZDT_Read();

    if(((ZDT_Pos_reset() != 1) || chassis.x != 0) && Init_flag == 0)
    {   
        return;
    }

    Init_flag = 1;

    ChassisControl(MOTOR_ACC);

    MecanumPosControl();
    
}